/**
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       chassis.c/h
  * @brief      2025赛季哨兵底盘控制任务
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *  V1.1.0     Nov-11-2019     RM              1. 增加底盘功率控制
  *  V2.0.0     Jan-1-2024      罗威昊           1. 添加小陀螺
  *  V3.0.0     Dec-20-2024     冉文治           1. 添加普通导航和变速小陀螺导航
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#include "chassis_task.h"
#include "chassis_behaviour.h"
#include "cmsis_os.h"
#include "arm_math.h"
#include "pid.h"
#include "remote_control.h"
#include "CAN_receive.h"
#include "detect_task.h"
#include "INS_task.h"
#include "gimbal_task.h"
#include "math.h"
#include "stdio.h"
#include "can.h"
#include "bsp_dwt.h"
#include "chassis_power_control.h"
#include "gimbal_behaviour.h"
#include "referee.h"

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

/**
  * @brief          底盘任务初始化
  * @author         冉文治
  * @param          chassis_move_init: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_init(chassis_move_t *chassis_move_init);

/**
  * @brief          设置底盘控制模式
  * @author         冉文治
  * @param          chassis_move_mode: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_set_mode(chassis_move_t *chassis_move_mode);

/**
  * @brief          底盘模式改变,有些参数需要改变
  * @author         冉文治
  * @param          chassis_move_transit: "chassis_move"变量指针
  * @retval         none
  */
void chassis_mode_change_control_transit(chassis_move_t *chassis_move_transit);

/**
  * @brief          底盘观测数据更新
  * @author         冉文治
  * @param          chassis_move_update: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_feedback_update(chassis_move_t *chassis_move_update);

/**
  * @brief          设置底盘控制设置值
  * @author         冉文治
  * @param          chassis_move_control: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_set_contorl(chassis_move_t *chassis_move_control);

/**
  * @brief          循环控制,根据设定值,计算电机电流,进行控制
  * @author         冉文治
  * @param          chassis_move_control_loop: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_control_loop(chassis_move_t *chassis_move_control_loop);

#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t chassis_high_water;
#endif

// 底盘运动数据
chassis_move_t chassis_move;

/**
  * @brief          底盘任务,间隔2ms
  * @author         冉文治
  * @param          pvParameters: 空
  * @retval         none
  */
void chassis_task(void const *pvParameters)
{
    // 空闲一段时间
    vTaskDelay(CHASSIS_TASK_INIT_TIME);
    // 底盘任务初始化
    chassis_init(&chassis_move);
    // 判断底盘电机是否在线
    while (toe_is_error(CHASSIS_MOTOR1_TOE) || toe_is_error(CHASSIS_MOTOR2_TOE) || toe_is_error(CHASSIS_MOTOR3_TOE) || toe_is_error(CHASSIS_MOTOR4_TOE) || toe_is_error(DBUS_TOE))
    {
        vTaskDelay(CHASSIS_CONTROL_TIME_MS);
    }

    while (1)
    {
        // 设置底盘控制模式
        chassis_set_mode(&chassis_move);
        // 模式切换数据保存
        chassis_mode_change_control_transit(&chassis_move);
        // 底盘数据更新
        chassis_feedback_update(&chassis_move);
        // 底盘控制量设置
        chassis_set_contorl(&chassis_move);
        // 底盘控制PID计算
        chassis_control_loop(&chassis_move);

        // 确保至少一个电机在线
        if (!(toe_is_error(CHASSIS_MOTOR1_TOE) && toe_is_error(CHASSIS_MOTOR2_TOE) && toe_is_error(CHASSIS_MOTOR3_TOE) && toe_is_error(CHASSIS_MOTOR4_TOE)))
        {
            // 当遥控器掉线的时候，发送给底盘电机电流为0
            if (toe_is_error(DBUS_TOE) && game_state_t.game_progress != 4)
            {
                CAN_cmd_chassis(0, 0, 0, 0);
                mit_ctrl_big_yaw_motor(&CHASSIS_CAN, chassis_move.chassis_radar_yaw_motor.gimbal_mit_motor_measure->id, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
            }
            else
            {
                CAN_cmd_chassis(chassis_move.motor_chassis[0].give_current, chassis_move.motor_chassis[1].give_current, chassis_move.motor_chassis[2].give_current, chassis_move.motor_chassis[3].give_current);
                if (chassis_move.chassis_state == CHASSIS_OFFLINE)
                {
                    enable_radar_yaw_motor_mode(&CHASSIS_CAN, chassis_move.chassis_radar_yaw_motor.gimbal_mit_motor_measure->id, chassis_move.chassis_radar_yaw_motor.gimbal_mit_motor_measure->mode);
                }
                else if (chassis_move.chassis_state == CHASSIS_ONLINE)
                {
                    // 发送控制电流
                    mit_ctrl_big_yaw_motor(&CHASSIS_CAN, chassis_move.chassis_radar_yaw_motor.gimbal_mit_motor_measure->id, 0.0f, 0.0f, 0.0f, 0.0f, -chassis_move.chassis_radar_yaw_motor.output_torque);
                }
            }
        }

        // 系统延时
        vTaskDelay(CHASSIS_CONTROL_TIME_MS);
#if INCLUDE_uxTaskGetStackHighWaterMark
        chassis_high_water = uxTaskGetStackHighWaterMark(NULL);
#endif
    }
}

/**
  * @brief          底盘任务初始化
  * @author         冉文治
  * @param          chassis_move_init: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_init(chassis_move_t *chassis_move_init)
{
    if (chassis_move_init == NULL)
    {
        return;
    }
		
    // 底盘速度PID
    const static fp32 motor_speed_pid[3] = {M3505_MOTOR_SPEED_PID_KP, M3505_MOTOR_SPEED_PID_KI, M3505_MOTOR_SPEED_PID_KD};
    // 底盘角度PID
    const static fp32 chassis_yaw_pid[3] = {CHASSIS_FOLLOW_GIMBAL_PID_KP, CHASSIS_FOLLOW_GIMBAL_PID_KI, CHASSIS_FOLLOW_GIMBAL_PID_KD};
    // X方向速度滤波参数
    const static fp32 chassis_x_order_filter[1] = {CHASSIS_ACCEL_X_NUM};
    // Y方向速度滤波参数
    const static fp32 chassis_y_order_filter[1] = {CHASSIS_ACCEL_Y_NUM};
    // 小陀螺速度转化参数
    chassis_move_init->chassis_spin_change_sen = CHASSIS_SPIN_LOW_SPEED;
    // 底盘开机状态为原始
    chassis_move_init->chassis_mode = CHASSIS_VECTOR_RAW;
    // 获得遥控器指针
    chassis_move_init->chassis_RC = get_remote_control_point();
    // 获得超级电容指针
    chassis_move_init->chassis_sup = get_supercap_measure_point();
    // 获得陀螺仪姿态角指针
    chassis_move_init->chassis_INS_angle = get_INS_angle_point();
    // 获得小yaw轴电机数据指针
    chassis_move_init->gimbal_yaw_motor = get_yaw_motor_point();
    // 获得pitch轴电机数据指针
    chassis_move_init->gimbal_pitch_motor = get_pitch_motor_point();
    // 获得雷达数据指针
    chassis_move_init->ld_ctrl = get_ld_point();
    // 获得机器人伤害类型指针
    chassis_move_init->robot_status = get_game_robot_status_point();
    chassis_move_init->robot_hurt = get_robot_hurt_point();
    // 获得底盘电机数据指针,初始化PID
    for (uint8_t i = 0; i < 4; i++)
    {
        chassis_move_init->motor_chassis[i].chassis_motor_measure = get_chassis_motor_measure_point(i);
        PID_init(&chassis_move_init->motor_speed_pid[i], PID_POSITION, motor_speed_pid, M3505_MOTOR_SPEED_PID_MAX_OUT, M3505_MOTOR_SPEED_PID_MAX_IOUT);
    }
    // 初始化角度PID
    PID_init(&chassis_move_init->chassis_angle_pid, PID_POSITION, chassis_yaw_pid, CHASSIS_FOLLOW_GIMBAL_PID_MAX_OUT, CHASSIS_FOLLOW_GIMBAL_PID_MAX_IOUT);
    //用一阶低通滤波代替斜波函数
    first_order_filter_init(&chassis_move_init->chassis_cmd_slow_set_vx, CHASSIS_CONTROL_TIME, chassis_x_order_filter);
    first_order_filter_init(&chassis_move_init->chassis_cmd_slow_set_vy, CHASSIS_CONTROL_TIME, chassis_y_order_filter);
    // 设置X轴和Y轴最大最小速度
    chassis_move_init->vx_max_speed = NORMAL_MAX_CHASSIS_SPEED_X;
    chassis_move_init->vx_min_speed = -NORMAL_MAX_CHASSIS_SPEED_X;
    chassis_move_init->vy_max_speed = NORMAL_MAX_CHASSIS_SPEED_Y;
    chassis_move_init->vy_min_speed = -NORMAL_MAX_CHASSIS_SPEED_Y;
		
    // 大yawPID值
    static const fp32 radar_yaw_motor_angle_pid[3] = {RADAR_YAW_MOTOR_ANGLE_PID_KP, RADAR_YAW_MOTOR_ANGLE_PID_KI, RADAR_YAW_MOTOR_ANGLE_PID_KD};
    // 初始化大yaw电机PID
    chassis_move_init->chassis_radar_yaw_motor.gimbal_mit_motor_measure = get_yaw_gimbal_mit_motor_measure_point();
    radar_yaw_motor_init();
    chassis_move_init->chassis_state = CHASSIS_OFFLINE;
	enable_radar_yaw_motor_mode(&CHASSIS_CAN, chassis_move_init->chassis_radar_yaw_motor.gimbal_mit_motor_measure->id, chassis_move_init->chassis_radar_yaw_motor.gimbal_mit_motor_measure->mode);
    vTaskDelay(1);
	init_radar_yaw_motor_mode(&CHASSIS_CAN, chassis_move_init->chassis_radar_yaw_motor.gimbal_mit_motor_measure->id, chassis_move_init->chassis_radar_yaw_motor.gimbal_mit_motor_measure->mode);
	vTaskDelay(1);
    PID_init(&chassis_move_init->chassis_radar_yaw_motor.chassis_radar_yaw_motor_angle_pid, PID_POSITION, radar_yaw_motor_angle_pid, RADAR_YAW_MOTOR_ANGLE_PID_MAX_OUT, RADAR_YAW_MOTOR_ANGLE_PID_MAX_IOUT);

    // 更新数据
    chassis_feedback_update(chassis_move_init);

}

/**
  * @brief          设置底盘控制模式
  * @author         冉文治
  * @param          chassis_move_mode: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_set_mode(chassis_move_t *chassis_move_mode)
{
    if (chassis_move_mode == NULL)
    {
        return;
    }

    // 设置底盘模式
    chassis_behaviour_mode_set(chassis_move_mode);
}

/**
  * @brief          底盘模式改变,有些参数需要改变
  * @author         冉文治
  * @param          chassis_move_transit: "chassis_move"变量指针
  * @retval         none
  */
void chassis_mode_change_control_transit(chassis_move_t *chassis_move_transit)
{
    if (chassis_move_transit == NULL)
    {
        return;
    }

    if (chassis_move_transit->last_chassis_mode == chassis_move_transit->chassis_mode)
    {
        return;
    }

    // 切入正常导航模式
    if ((chassis_move_transit->last_chassis_mode != CHASSIS_AUTO_FOLLOW_RADAR_YAW) && chassis_move_transit->chassis_mode == CHASSIS_AUTO_FOLLOW_RADAR_YAW)
    {			
        chassis_move_transit->chassis_relative_angle_set = 0.0f;
        chassis_move_transit->chassis_radar_yaw_angle_set = chassis_move_transit->chassis_radar_angle;
    }
    // 切入小陀螺导航模式
    else if ((chassis_move_transit->last_chassis_mode != CHASSIS_AUTO_ROTATION) && chassis_move_transit->chassis_mode == CHASSIS_AUTO_ROTATION)
    {
        chassis_move_transit->chassis_relative_angle_set = 0.0f;
        chassis_move_transit->chassis_radar_yaw_angle_set = chassis_move_transit->chassis_radar_angle;
    }
    // 切入底盘挂机模式
    else if ((chassis_move_transit->last_chassis_mode != CHASSIS_VECTOR_NO_FOLLOW_YAW) && chassis_move_transit->chassis_mode == CHASSIS_VECTOR_NO_FOLLOW_YAW)
    {
        chassis_move_transit->chassis_relative_angle_set = 0.0f;
    }
    // 切入遥控模式
    else if ((chassis_move_transit->last_chassis_mode != CHASSIS_VECTOR_FOLLOW_GIMBAL_YAW) && chassis_move_transit->chassis_mode == CHASSIS_VECTOR_FOLLOW_GIMBAL_YAW)
    {			
        chassis_move_transit->chassis_relative_angle_set = 0.0f;
    }
		else if ((chassis_move_transit->last_chassis_mode != CHASSIS_VECTOR_ROTATION) && chassis_move_transit->chassis_mode == CHASSIS_VECTOR_ROTATION)
    {			
        chassis_move_transit->chassis_relative_angle_set = 0.0f;
    }

    chassis_move_transit->last_chassis_mode = chassis_move_transit->chassis_mode;
}

/**
  * @brief          底盘观测数据更新
  * @author         冉文治
  * @param          chassis_move_update: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_feedback_update(chassis_move_t *chassis_move_update)
{
    if (chassis_move_update == NULL)
    {
        return;
    }

    // dwt计时器更新
    chassis_move_update->dt = DWT_GetDeltaT(&chassis_move_update->chassis_dwt_count);
    
    // 更新电机速度
    for (uint8_t i = 0; i < 4; i++)
    {
        chassis_move_update->motor_chassis[i].speed = CHASSIS_MOTOR_RPM_TO_VECTOR_SEN * chassis_move_update->motor_chassis[i].chassis_motor_measure->speed_rpm;
        chassis_move_update->motor_chassis[i].accel = chassis_move_update->motor_speed_pid[i].Dbuf[0] * CHASSIS_CONTROL_FREQUENCE;
    }
    // 更新底盘纵向速度x,水平速度y,旋转速度wz
    chassis_move_update->vx = (-chassis_move_update->motor_chassis[0].speed + chassis_move_update->motor_chassis[1].speed + chassis_move_update->motor_chassis[2].speed - chassis_move_update->motor_chassis[3].speed) * invSqrt(2) / 2 * MOTOR_SPEED_TO_CHASSIS_SPEED_VX;
    chassis_move_update->vy = (-chassis_move_update->motor_chassis[0].speed - chassis_move_update->motor_chassis[1].speed + chassis_move_update->motor_chassis[2].speed + chassis_move_update->motor_chassis[3].speed) * invSqrt(2) / 2 * MOTOR_SPEED_TO_CHASSIS_SPEED_VY;
    chassis_move_update->wz = (-chassis_move_update->motor_chassis[0].speed - chassis_move_update->motor_chassis[1].speed - chassis_move_update->motor_chassis[2].speed - chassis_move_update->motor_chassis[3].speed) * MOTOR_SPEED_TO_CHASSIS_SPEED_WZ / MOTOR_DISTANCE_TO_CENTER;
    // 更新底盘姿态角度
    chassis_move_update->chassis_yaw = rad_format(chassis_move_update->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos - rad_format(*(chassis_move_update->chassis_INS_angle + INS_YAW_ADDRESS_OFFSET) - chassis_move_update->gimbal_yaw_motor->relative_angle));
    chassis_move_update->chassis_pitch = rad_format(*(chassis_move_update->chassis_INS_angle + INS_PITCH_ADDRESS_OFFSET) - chassis_move_update->gimbal_pitch_motor->relative_angle);
    chassis_move_update->chassis_roll = *(chassis_move_update->chassis_INS_angle + INS_ROLL_ADDRESS_OFFSET);

    chassis_move_update->chassis_radar_angle = rad_format(chassis_move_update->gimbal_yaw_motor->relative_angle - chassis_move_update->gimbal_yaw_motor->absolute_angle);
    if (gimbal_behaviour == GIMBAL_CV || gimbal_behaviour == GIMBAL_ABSOLUTE_ANGLE)
    {
        if (chassis_move_update->gimbal_yaw_motor->relative_angle >= PI / 2)
        {
            chassis_move_update->chassis_radar_yaw_add_angle_set = -chassis_move_update->dt;
        }
        else if (chassis_move_update->gimbal_yaw_motor->relative_angle <= -PI / 2)
        {
            chassis_move_update->chassis_radar_yaw_add_angle_set = chassis_move_update->dt;
        }
    }
    else
    {
        chassis_move_update->chassis_radar_yaw_add_angle_set = chassis_move_update->dt;
    }

    chassis_move_update->last_robot_HP = chassis_move_update->robot_status->current_HP;
}

/**
  * @brief          根据遥控器通道值,计算纵向和横向移动速度
  * @author         冉文治
  * @param          vx_set: 纵向速度指针
  * @param          vy_set: 横向速度指针
  * @param          chassis_move_rc_to_vector: "chassis_move"变量指针
  * @retval         none
  */
void chassis_rc_to_control_vector(fp32 *vx_set, fp32 *vy_set, chassis_move_t *chassis_move_rc_to_vector)
{
    if (chassis_move_rc_to_vector == NULL || vx_set == NULL || vy_set == NULL)
    {
        return;
    }
    
    int16_t vx_channel, vy_channel;
    fp32 vx_set_channel, vy_set_channel;
    // 死区限制
    rc_deadband_limit(chassis_move_rc_to_vector->chassis_RC->rc.ch[CHASSIS_X_CHANNEL], vx_channel, CHASSIS_RC_DEADLINE);
    rc_deadband_limit(chassis_move_rc_to_vector->chassis_RC->rc.ch[CHASSIS_Y_CHANNEL], vy_channel, CHASSIS_RC_DEADLINE);
    // 速度转换
    vx_set_channel = vx_channel * CHASSIS_VX_RC_SEN;
    vy_set_channel = vy_channel * -CHASSIS_VY_RC_SEN;
    // 用一阶低通滤波代替斜波函数作为底盘速度输入
    first_order_filter_cali(&chassis_move_rc_to_vector->chassis_cmd_slow_set_vx, vx_set_channel);
    first_order_filter_cali(&chassis_move_rc_to_vector->chassis_cmd_slow_set_vy, vy_set_channel);
    // 停止信号
    if (vx_set_channel < CHASSIS_RC_DEADLINE * CHASSIS_VX_RC_SEN && vx_set_channel > -CHASSIS_RC_DEADLINE * CHASSIS_VX_RC_SEN)
    {
        chassis_move_rc_to_vector->chassis_cmd_slow_set_vx.out = 0.0f;
    }

    if (vy_set_channel < CHASSIS_RC_DEADLINE * CHASSIS_VY_RC_SEN && vy_set_channel > -CHASSIS_RC_DEADLINE * CHASSIS_VY_RC_SEN)
    {
        chassis_move_rc_to_vector->chassis_cmd_slow_set_vy.out = 0.0f;
    }
    // 纵向和横向速度输出
    *vx_set = chassis_move_rc_to_vector->chassis_cmd_slow_set_vx.out;
    *vy_set = chassis_move_rc_to_vector->chassis_cmd_slow_set_vy.out;
}

/**
  * @brief          设置底盘控制设置值
  * @author         冉文治
  * @param          chassis_move_control: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_set_contorl(chassis_move_t *chassis_move_control)
{

    if (chassis_move_control == NULL)
    {
        return;
    }

    fp32 vx_set = 0.0f, vy_set = 0.0f, angle_set = 0.0f;
    // 获得三个设置值
    chassis_behaviour_control_set(&vx_set, &vy_set, &angle_set, chassis_move_control);

    // 遥控模式,即底盘跟随云台
    if (chassis_move_control->chassis_mode == CHASSIS_VECTOR_FOLLOW_GIMBAL_YAW)
    {
        fp32 sin_yaw = 0.0f, cos_yaw = 0.0f;
        sin_yaw = arm_sin_f32(-chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos);
        cos_yaw = arm_cos_f32(-chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos);
        // 将纵向速度和横向速度正交分解
		chassis_move_control->vx_set = cos_yaw * vx_set + sin_yaw * vy_set;
        chassis_move_control->vy_set = -sin_yaw * vx_set + cos_yaw * vy_set;
        // 设置底盘相对角度,此模式下为0
        chassis_move_control->chassis_relative_angle_set = angle_set;
        // 计算旋转PID角速度
        chassis_move_control->wz_set = PID_calc(&chassis_move_control->chassis_angle_pid, chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos, chassis_move_control->chassis_relative_angle_set);
        // 雷达yaw轴电机角度环PID
        chassis_move_control->chassis_radar_yaw_motor.output_torque = PID_calc(&chassis_move_control->chassis_radar_yaw_motor.chassis_radar_yaw_motor_angle_pid, chassis_move_control->gimbal_yaw_motor->relative_angle, 0);
        // 速度限幅
        chassis_move_control->vx_set = fp32_constrain(chassis_move_control->vx_set, chassis_move_control->vx_min_speed, chassis_move_control->vx_max_speed);
        chassis_move_control->vy_set = fp32_constrain(chassis_move_control->vy_set, chassis_move_control->vy_min_speed, chassis_move_control->vy_max_speed);
    }
	// 遥控小陀螺模式
	else if (chassis_move_control->chassis_mode == CHASSIS_VECTOR_ROTATION)
	{
		fp32 sin_yaw = 0.0f, cos_yaw = 0.0f;
        // 计算底盘相对大yaw的角度的三角函数
        sin_yaw = arm_sin_f32(-chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos);
        cos_yaw = arm_cos_f32(-chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos);
        // 将纵向速度和横向速度正交分解
		chassis_move_control->vx_set = cos_yaw * vx_set + sin_yaw * vy_set;
        chassis_move_control->vy_set = -sin_yaw * vx_set + cos_yaw * vy_set;
        // 设置小陀螺的角速度
        fp32 chassis_wz = angle_set;
        // 将小陀螺角速度进行转化,并加入变速陀螺
        chassis_move_control->wz_set = chassis_wz * chassis_move_control->chassis_spin_change_sen * (-0.02533029591058444286096986580243f * chassis_move_control->chassis_yaw * chassis_move_control->chassis_yaw + 1.0f);
        // 雷达yaw轴电机角度环PID
		chassis_move_control->chassis_radar_yaw_motor.output_torque = PID_calc(&chassis_move_control->chassis_radar_yaw_motor.chassis_radar_yaw_motor_angle_pid, chassis_move_control->gimbal_yaw_motor->relative_angle, 0);
        // 速度限幅
        chassis_move_control->vx_set = fp32_constrain(chassis_move_control->vx_set, chassis_move_control->vx_min_speed, chassis_move_control->vx_max_speed);// + 0.09f;
        chassis_move_control->vy_set = fp32_constrain(chassis_move_control->vy_set, chassis_move_control->vy_min_speed, chassis_move_control->vy_max_speed);// + 0.07f;
	}
    // 底盘挂机模式
    else if (chassis_move_control->chassis_mode == CHASSIS_VECTOR_NO_FOLLOW_YAW)
    {
        // 设置底盘相对角度,此模式下为0
        chassis_move_control->wz_set = angle_set;
        chassis_move_control->chassis_radar_yaw_motor.output_torque = 0.0f;
        // 速度限幅
        chassis_move_control->vx_set = fp32_constrain(vx_set, chassis_move_control->vx_min_speed, chassis_move_control->vx_max_speed);
        chassis_move_control->vy_set = fp32_constrain(vy_set, chassis_move_control->vy_min_speed, chassis_move_control->vy_max_speed);
    }
    // 底盘初始化模式
    else if (chassis_move_control->chassis_mode == CHASSIS_VECTOR_RAW)
    {
        // 所有设定值均为0
        chassis_move_control->vx_set = vx_set;
        chassis_move_control->vy_set = vy_set;
        chassis_move_control->wz_set = angle_set;
        chassis_move_control->chassis_radar_yaw_motor.output_torque = 0.0f;
        chassis_move_control->chassis_cmd_slow_set_vx.out = 0.0f;
        chassis_move_control->chassis_cmd_slow_set_vy.out = 0.0f;
    }
    // 小陀螺导航模式
    else if (chassis_move_control->chassis_mode == CHASSIS_AUTO_ROTATION)
    {
        fp32 sin_yaw = 0.0f, cos_yaw = 0.0f;
        // 计算底盘相对大yaw的角度的三角函数
        sin_yaw = arm_sin_f32(-chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos);
        cos_yaw = arm_cos_f32(-chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos);
        // 将纵向速度和横向速度正交分解
		chassis_move_control->vx_set = cos_yaw * vx_set + sin_yaw * vy_set;
        chassis_move_control->vy_set = -sin_yaw * vx_set + cos_yaw * vy_set;
        // 设置小陀螺的角速度
        fp32 chassis_wz = angle_set;
        // 将小陀螺角速度进行转化,并加入变速陀螺
        chassis_move_control->wz_set = chassis_wz * chassis_move_control->chassis_spin_change_sen * (-0.02533029591058444286096986580243f * chassis_move_control->chassis_yaw * chassis_move_control->chassis_yaw + 1.0f);
        // 雷达yaw轴电机角度环PID
        // 雷达yaw轴角度设置
        chassis_move_control->chassis_radar_yaw_angle_set += chassis_move_control->chassis_radar_yaw_add_angle_set;
        chassis_move_control->chassis_radar_yaw_motor.output_torque = PID_calc(&chassis_move_control->chassis_radar_yaw_motor.chassis_radar_yaw_motor_angle_pid,
                                                                        rad_format(chassis_move_control->chassis_radar_angle - rad_format(chassis_move_control->chassis_radar_yaw_angle_set)),
                                                                        0.0f);
        // 速度限幅
        chassis_move_control->vx_set = fp32_constrain(chassis_move_control->vx_set, chassis_move_control->vx_min_speed, chassis_move_control->vx_max_speed);// + 0.09f;
        chassis_move_control->vy_set = fp32_constrain(chassis_move_control->vy_set, chassis_move_control->vy_min_speed, chassis_move_control->vy_max_speed);// + 0.07f;
    }
    // 正常导航模式
    else if(chassis_move_control->chassis_mode == CHASSIS_AUTO_FOLLOW_RADAR_YAW)
    {
        fp32 sin_yaw = 0.0f, cos_yaw = 0.0f;
        // 计算底盘相对大yaw的角度的三角函数
        sin_yaw = arm_sin_f32(-chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos);
        cos_yaw = arm_cos_f32(-chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos);
        // 将纵向速度和横向速度正交分解
		chassis_move_control->vx_set = cos_yaw * vx_set + sin_yaw * vy_set;
        chassis_move_control->vy_set = -sin_yaw * vx_set + cos_yaw * vy_set;
        // 设置底盘角度，方向与导航方向有关
        chassis_move_control->chassis_relative_angle_set = rad_format(angle_set);
        // 计算旋转PID角速度
        if (chassis_move_control->vx_set != 0.0f ||  chassis_move_control->vy_set != 0.0f)
        {
            chassis_move_control->wz_set = PID_calc(&chassis_move_control->chassis_angle_pid, rad_format(chassis_move_control->chassis_radar_yaw_motor.gimbal_mit_motor_measure->pos - chassis_move_control->chassis_relative_angle_set), 0);
        }
        else
        {
            chassis_move_control->wz_set = 0.0f;
        }
        // 雷达yaw轴电机角度环PID
        // 雷达yaw轴角度设置
        chassis_move_control->chassis_radar_yaw_angle_set += chassis_move_control->chassis_radar_yaw_add_angle_set;
        chassis_move_control->chassis_radar_yaw_motor.output_torque = PID_calc(&chassis_move_control->chassis_radar_yaw_motor.chassis_radar_yaw_motor_angle_pid,
                                                                                rad_format(chassis_move_control->chassis_radar_angle - rad_format(chassis_move_control->chassis_radar_yaw_angle_set)),
                                                                                0.0f);
        // 速度限幅
        chassis_move_control->vx_set = fp32_constrain(chassis_move_control->vx_set, chassis_move_control->vx_min_speed, chassis_move_control->vx_max_speed);
        chassis_move_control->vy_set = fp32_constrain(chassis_move_control->vy_set, chassis_move_control->vy_min_speed, chassis_move_control->vy_max_speed);
    }
}

/**
  * @brief          计算四个轮子速度
  * @author         冉文治
  * @param          vx_set: 纵向速度
  * @param          vy_set: 横向速度
  * @param          wz_set: 旋转速度
  * @param          wheel_speed: 四个轮子速度
  * @retval         none
  */
static void chassis_vector_to_mecanum_wheel_speed(const fp32 vx_set, const fp32 vy_set, const fp32 wz_set, fp32 wheel_speed[4])
{
    wheel_speed[0] = -vx_set - vy_set + MOTOR_DISTANCE_TO_CENTER * wz_set;
    wheel_speed[1] = vx_set - vy_set + MOTOR_DISTANCE_TO_CENTER * wz_set;
    wheel_speed[2] = vx_set + vy_set + MOTOR_DISTANCE_TO_CENTER * wz_set;
    wheel_speed[3] = -vx_set + vy_set + MOTOR_DISTANCE_TO_CENTER * wz_set;
}

/**
  * @brief          循环控制,根据设定值,计算电机电流,进行控制
  * @author         冉文治
  * @param          chassis_move_control_loop: "chassis_move"变量指针
  * @retval         none
  */
static void chassis_control_loop(chassis_move_t *chassis_move_control_loop)
{
    fp32 max_vector = 0.0f, vector_rate = 0.0f;
    fp32 temp = 0.0f;
    fp32 wheel_speed[4] = {0.0f, 0.0f, 0.0f, 0.0f};

    // 速度分解
    chassis_vector_to_mecanum_wheel_speed(chassis_move_control_loop->vx_set, chassis_move_control_loop->vy_set, chassis_move_control_loop->wz_set, wheel_speed);

    // 初始化模式直接跳出函数
    if (chassis_move_control_loop->chassis_mode == CHASSIS_VECTOR_RAW)
    {
        
        for (uint8_t i = 0; i < 4; i++)
        {
            chassis_move_control_loop->motor_chassis[i].give_current = (int16_t)(wheel_speed[i]);
        }
        return;
    }

    // 计算轮子控制最大速度,并限制其最大速度
    for (uint8_t i = 0; i < 4; i++)
    {
        chassis_move_control_loop->motor_chassis[i].speed_set = wheel_speed[i];
        temp = fabs(chassis_move_control_loop->motor_chassis[i].speed_set);
        if (max_vector < temp)
        {
            max_vector = temp;
        }
    }

    if (max_vector > MAX_WHEEL_SPEED)
    {
        vector_rate = MAX_WHEEL_SPEED / max_vector;
        for (uint8_t i = 0; i < 4; i++)
        {
            chassis_move_control_loop->motor_chassis[i].speed_set *= vector_rate;
        }
    }

    // 计算PID
    for (uint8_t i = 0; i < 4; i++)
    {
        PID_calc(&chassis_move_control_loop->motor_speed_pid[i], chassis_move_control_loop->motor_chassis[i].speed, chassis_move_control_loop->motor_chassis[i].speed_set);
    }
		
		chassis_power_control(chassis_move_control_loop);

    // 赋值电流值
    for (uint8_t i = 0; i < 4; i++)
    {
        chassis_move_control_loop->motor_chassis[i].give_current = (int16_t)(chassis_move_control_loop->motor_speed_pid[i].out);
    }
}
